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ABSTRACT 


A  three-dimensional  scene,  such  as  a  proposed 
building,  an  Imaginary  landscape,  or  an  organic  molecule, 
is  selected,  described  in  abstract  terms,  and  stored  in  a 
computer's  memory,  A  person  wears  a  special  helmet,  in  a 
laboratory  whose  inner  wall  is  dotted  with  “landmark" 
LEDs.  The  helmet  is  equipped  with  a  location  system  and  a 
projection  system.  As  the  wearer  moves  in  the  laboratory, 
changing  the  helmet’s  position  (and  orientation)  in  a 
natural  manner,  the  location  system  allows  the  computer 
to  keep  track  of  the  helmet’s  position,  and  the  computer 
sends  appropriate  information  to  the  projection  system, 
to  display  the  view  of  the  selected  scene  (suitably  scaled) 
that  would  be  seen  by  the  wearer  during  this  motion. 

The  geometry  of  an  arbitrarily-arranged,  three- 
camera,  headmounted  location  system  for  identifying  the 
position  and  orientation  of  the  helmet,  relative  to 
“landmark"  pinpoint-LEDs  distributed  over  the  inside 
wall  of  the  laboratory,  Is  described  in  mathematical 
terms.  A  fast  Newton-type  method  for  performing  the 
required  positional  computations,  is  presented  and 
evaluated. 


General  Headraount  Geometry 


1.  Introduction 


The  emotional  appeal  and  practical  usefulness  of  being  able  to 
experience  what  it  is  like  to  be  somewhere  without  actually  going 
there  (the  jungles  of  New  Guinea  or  Ecuador,  the  top  of  Everest,  the 
bottom  of  the  Mindanao  Trench,  the  craters  of  the  Moon,  the  rooms 
and  hallways  of  a  proposed  building,  the  landscape  of  a  science-fiction 
adventure,  the  inside  of  a  watch,  a  protein  molecule,  .  .  .  )  is  evident. 
The  entire  film  and  television  industry  attests  to  the  idea’s  popular 
interest,  and  the  advantages  to  the  tr  rinee-explorer,  the  anatomist,  or 
the  molecular  biologist  are  clearly  great.  But  the  viewer  of  a  movie  is 
entirely  passive,  compelled  to  follow  the  motions  of  the  photographer 
or  animator,  as  the  latter  chooses  the  point  of  view.  Some  success  in 
allowing  the  viewer  to  select  and  move  his  or  her  point  of  view  has 
been  achieved  by  projecting  three-dimensional  holographic  images; 
but  now  the  scene  to  be  displayed  is  very  much  restricted  in  space. 

The  subject  of  the  present  work  is  an  alternative  approach,  in 
which  the  viewer  walks  or  rides  around  a  laboratory,  as  a  picture  of  the 
selected  scene  is  projecte  J  for  him  or  her,  related  to  the  position  and 
orientation  of  his  or  her  ..cad,  as  if  the  laboratory  were  magically 
transformed  into  the  abstract  scene.  The  scene  can  easily  be  scaled, 
so  that  several  meters  of  movement  can  represent  either  many 
thousands  of  light-years  of  intergalactic  space  or  a  tiny  fraction  of  a 
micrometer  along  the  surface  of  a  virus  molecule.  To  achieve  this 
effect,  the  viewer  wears  a  helmet,  on  which  are  mounted  a  location 
system  and  a  projection  system.  The  inner  walls  of  the  laboratory  are 
dotted  with  “landmark”  infra-red  leds,  whose  positions  are  precisely 
known,  and  the  location  system  detects  the  identities,  and  the 
positions  relative  to  the  helmet,  of  at  least  three  of  these  leds,  so  as  to 
determine  the  wearer’s  position  and  orientation  in  the  laboratory  (and 
therefore  in  the  selected  scene).  Practical  considerations,  connected 
with  the  "engineering  balance”  between  directional  sensitivity  and 
breadth  of  vision,  dictate  that  the  location  system  should  consist  of 
three  cameras,  rather  than  one.  The  leds  are  “lit”  briefly,  one  at  a 
time,  in  a  cyclic  manner;  so  that  the  location  system  identifies  them 
by  the  time  at  which  they  are  seen. 

We  are  not  concerned,  here,  with  the  detailed  specifications  of 
the  equipment,  or  with  the  working  of  the  projection  system.  The 
focus  of  the  present  work  is  to  obtain  and  analyze  a  method  whereby 
the  angular  readings  of  direction,  relative  to  the  helmet,  obtained  by 
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the  cameras,  are  combined  with  the  known  positions  of  the  leds,  to 
yield  the  position  and  orientation  of  the  head-mounted  system  in  the 
laboratory.  This  work  is  similar  in  spirit  to  that  of  E.  Church  (1945. 
1948);  but  the  latter  was  limited  to  the  considerably  simpler  case  of  a 
single  camera,  not  practically  useful  here,  but  appropriate  to  work  in 
aerial  photography,  for  example. 

The  general  problem  considered  here  leads  to  quite 
complicated  equations,  simplified  only  by  their  considerable 
symmetries.  To  make  them  manageable,  vector  transformations  which 
may  be  unfamiliar  to  the  reader  are  needed.  These  and  various  other 
mathematical  details  are  collected  in  the  Appendix,  for  the  reader’s 
convenience. 

To  fix  the  practical  considerations,  we  note  that  some  300 
readings  per  second  can  be  made  and  recorded  by  the  cameras  in  the 
computer  (this  can  be  increased  as  much  as  fivefold).  Angular 

deflections  of  as  much  as,  say  60°  in  g-  second,  are  possible;  so  that,  for 

successive  readings,  the  computer  will  have  to  cope  with  angular 
deflections  of  up  to  1°  of  arc  (translational  movement  is  likely  to  be 
less  abrupt). 


2.  The  General  Three-Camera  System 


We  consider  the  general  geometry  shown  in  Figure  1.  Let  O  be 
the  origin  of  coordinates.  The  origin,  S,  of  the  headmounted  camera 
system  is  at  (column)  vector  position  s  from  O.  The  optical  centers  of 
the  three  camera  lenses  are  at  TT.  V,  and  W,  at  vector  positions  u.  v, 
and  w,  respectively,  from  S.  Sh  :e  ;he  headmounted  system  is  rigidly 
configured,  it  is  clear  that,  in  any  x  ition, 

"uT1  [u  v  wj  =  T  if  u.v  u.w  1  =  I"  a  f  e 

vT  v.u  if  v.w  J  b  d  ,  (2.1) 

-tvTJ  L  tv.  u  w.v  up-  J  L  e  d  c  _ 

where  [see  (Al)  -  (A8)  in  the  Appendix  to  this  paper]  (i)  xT  is  the 
transposed  (row)  vector  with  the  same  components  as  the  column 
vector  x,  (ii)  x  .  y  denotes  the  scalar  product  of  vectors  x  and  y  (in 
matrix  notation,  x  .  y  -  xTy).  (iii)  x2  =  x  .  x.  The  parameters  a.  b.  c,  d. 
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e,  and  /  are  given  by  the  geometry  of  the  headmounted  system  (with 
a.  £>.  and  c  positive)..  In  other  words,  in  terms  of  a  “world  coordinate 
frame"  of  reference  {{,  j,  k),  say,  if 

u  =  i  +  U2J+  U3  k 

v  =  Uj  i+  v2j+  Uj  k 
w  =  wli  +  IU2J+  ujg  k 


(2.3) 


Figure  1. 


General  Keadmount  Geometry 


then 


u2  =  Uj2  +  U22  +  U32  =  a  ' 

v2  -  vf  +  U22  +  U32  =  b  >, 

tu2  =  +  W22  +  w^2  =  c 

V  .  W  =  W  .  V  =  U1UJ1  +  V2W2  +  U3LU3  =  d  ' 

IL'.U  =  U.W  =  LOjlij  +  U^U2  +  LUjUg  =  e  >. 

U  .  I?  =  U.U  =  +  U3U3  =  /  , 


(2.4a) 


(2.4b) 


The  “landmarks"  (LEDs),  A,  B,  and  C,  are  respectively  sighted  from 
the  cameras  at  U,  V,  and  W,  in  the  directions  of  vectors  p,  q,  and  r. 
However,  only  the  directions  of  these  vectors  are  significant  (the 
magnitudes  are  arbitrary).  We  note  that  p,  q,  and  r  will  be  determined 
relative  to  their  respective  cameras’  orientations.  Thus,  by  suitably 
transforming  the  raw  experimental  observations,  we  may  view  these 
(column)  vectors  as  satisfying  a  relation  of  the  form 


p  ~  knu  +  k2lv  +  k3lw' 

q  =  kl2u+k22V+k32w  f, 
r  =  k13  u  +  k23  v  +  /C33  w  , 


(2.5) 


where  the  nine  components  kg  of  the  square  matrix  K  (which  will  be 
invertible  so  long  as  the  vectors  (u,  v,  u;)  form  a  base )  are  all  known 
even  though  u,  v,  and  w  are  net.  Equation  (2.5)  may  be  written  as 

[p  q  r]  =  [u  v  w]  K,  (2.6) 

Note  that,  since  K  is  invertible  (with  reciprocal  K~l  =  H,  say),  (2.5)  or 
(2.6)  implies  that 


[u  v  w]  =  [p  q  r]  H; 

u  =  hup+h21q+hQ1  r  ' 
v  =  h12p+  h22q+  hg2r  > 
w  =  h13p+  h23q+  h33r\ 


(2.7) 


(2.8) 
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Now.  we  are  given  the  actual  spatial  positions — relative  to  the 
origin  O — of  A.  B,  and  O;  namely,  A,  B,  and  C.  respectively.  Thus,  we 
know  that  multipliers  X,  p.  and  v  exist,  for  which 


s  +  u  +  Xp  =  A 
s+v  +  pq  =  B 
s+  w+  vr  =  C  j 


~sf 

"V 

'Bf 

“Cf 

If  s  = 

S2 

.  A  = 

A2 

,  B  = 

% 

.  C  = 

s> 

-S3- 

-A3- 

-S3- 

then  (2.9)  becomes 


s,  +  Uj  +  X px 
s2  +  U2  +  Xp2 
s3  +  Ug  +  Xp3 

sl  +  vl  +  pql 

s2  +  v2  + 
s3  +  u3  +  pq3 


=  A, 


~  A2 


=  A 


'3  J 


=  B, 


=  ^2 


=  B. 


3  J 


Si  +  U>1  +  VTj  =  Cj 

s2  +  w2  +  ^2  =  p2 

s3  +  +  vr3  =  C3  J 


(2.9) 


(2.10) 


(2.11a) 


(2.1!b) 


(2.11c) 


There  are  thus  altogether  15  scalar  unknowns;  namely,  X,  p,  v,  and  the 
components  of  the  four  3-vectors  s,  u,  v,  and  w.  To  determine  these, 
we  have  six  independent  equations  in  (2.4)  and  nine  equations  in 
(2.11). 

We  can  now  proceed  by  eliminating,  first,  X,  p,  and  v,  and  then 
Sj,  s2.  and  s3  from  the  equations  in  (2.11),  to  yield  three  equations; 
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but  a  purely  vectorial  approach  gives  us  the  same  results  more  directly. 
First,  we  vector-multiply  the  equations  in  (2.9)  by  p,  q,  and  r, 
respectively,  to  eliminate  from  these  equations  the  unneeded 

parameters  A,  p,  and  v: 


SAP  =  ( A  -  li)  A  p 

s  a  q  -  (B  -  v)  a  q  r , 
sap  =  (C  -  to)  a  r , 


(2.12) 


where  [see  (A9)  -  (A14)  in  the  Appendix]  x  a  y  denotes  the  anti- 
commutative  vector  product  of  x  and  y.  Then  we  scalar-multiply  the 
equations  in  (2.12)  by  q  and  r,  r  and  p,  and  p  and  q,  respectively 
(using  (A15)  and  (A16)  of  the  Appendix],  to  eliminate  s: 


Is  q  r  !  =  i(B-u)  q  r  I  =  I  (C  -  ud  q  r| 

Is  rp|  =  !  (C-it)  r  pi  =  |  (A -id  r  p|  h 

Is  p  q  |  =  I  (A -id  p  q  I  =  I  (B  -  id  p  q|  , 


(2.13) 


Using  (A17) — Fact  4 — of  the  Appendix,  and  (2.13),  we  see  that 


!  (B  -  u)  q  r  |  (C-io)  r  p  |  (A-u)  p  q| 

s  _  — - p  +  - - q  +  - - r, 

p  q  r  p  q  r  p  q  r 


(2.14) 


which  will  enable  us  to  recover  s  when  we  know  u,  v,  w,  p.  q.  and  r. 
Now,  by  (2.5)  with  (All),  we  see  (hat 


q  a  r  =  [kl2  u  +  l<22  v  +  k32  w)  A  (kl3  u  +  k23  v  +  k33  w) 

=  ^22^33  “  ^23^32^  v  A  w  +  ^32^13  ~  ^33^12^  w  A  u 
+  [kl2k2 3  -  kl3k22)  u  a  v.  (2. 1  5) 

Hence  and  by  analogy,  using  (A24)  and  (A26),  we  see  that 
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q  a  r 
r  a  p 
P  a  q 


lK|(hn  v  a  w  +  hl2  W  A  U  +  hj3  u  a  y) 

Ixl  (hjj  VA  UI+  ^2  ^  A  U  +  ^  A  l?) 

|  K'j  (hg,  VAW  +  hg2  U>  A  U  +  hgg  UA  Uj 


(2.16) 


Since,  by  (A15).  |  x  y  z  |  =  x.  [y  a  z); 

it  follows  from  (2.16)  that 

I  x  q  r|  =  |k|  x .  (hn  v  a  w+  h12  w  a  u+  h13  u  a  y) 

I  y  r  p  |  =  |k|  y .  (h21  y  a  u;+ ^  it;  a  u+ h23  u  a  r) 

I  2  p  q  |  =  |k(  z  .  (h31  y  a  u?+ h32  u;  a  u  +  u  a  y) 


(2.17) 


>.  (2.18) 


The  equations  in  (2.13)  not  involving  s  are 

I  (B-C+w-v)  q  r!  =  0 
I  (C-A+u-iy)  r  p  I  =  0 
1  ( A  -  J3  +  y  -  u)  p  q  |  =  0 , 


(2.19) 


and,  with  (2.18),  these  yield 


I  K|  (B  -  C  +  ty  -  y)  .  (hx  1  v  a  w  +  h12  ty  a  u  +  h13  u  a  y)  =  0  ^ 

IkI  (C-A  +  u-ui)  .  (/^j  ua  iy+  wau  +  u  a  v)  =  0 

|k|  (A  -  B  +  y  -  u)  .  (hg,  y  a  iy  +  hg?  wau+  hgg  u  a  y)  =  0 


(2.20) 


If  we  write  I  u  y  to  |  =4,  (2.21) 

we  see  that  (since  the  determinant  of  a  product  of  matrices  equals  the 
product  of  the  determinants  of  the  factor  matrices,  and  since 
determinants  are  not  altered  by  transposition),  by  (2.1),  A  will  satisfy 
the  equation 


-8- 


General  Headmount  Geometry 


d2 


U  V  w  1 2  = 


a  d  f 
d  b  e 
fee 


(2.22) 


and,  further,  by  (2.6), 


p  q  r  |  =  a\k 


(2.23) 


Let  us  also  write 


Dj  =  I  (B-O  v  w  |, 

E1  =  I  (C-A)  v  tu  I, 

Fj  =  I  (A-B)  u  u)  I, 


=  |  (B  -  Q  tu  u  | , 

£<2  =  I  (C-A)  w  u\, 

F2  =  |  (A-B)  tu  u  |, 


d3  = 

1  (B- 

-Q 

u 

| 

v  1 

*3  = 

1  (C- 

-A) 

u 

v  I  > 

^3  = 

1  (A- 

-Bl 

u 

v  L 

(2.24) 


Then  (2.20)  simplifies  to 


ftnDi  +  hl2D2  +  ^i3°3  -  4(^12  ”  ^13^ 
^21^1  +  ^22^2  +  ^23^3  =  ^^23  “  ^21^  f  ■ 


^31^1  +  ^32^2  +  ^33^3  =  ^2^  J 


(2.25) 


3.  Newton’s  Method 


We  see.  by  (2.24),  that  equations  (2.4J  and  (2.25)  are  quadratic  in  the 
components  of  u,  v.  and  w,  all  the  coefficients  being  known  (or,  at 
least,  easily  computed).  It  is  easiest  to  solve  these  equations  by 
Newton's  method.  We  begin  the  computation  with  an  initial 
approximation,  (u^.  u*0*,  w^J.  which  can,  for  instance,  be  the  solution 
obtained  (by  Newton’s  method)  for  the  most  recent  set  of 
observations,  as  the  wearer  of  the  headmounted  system  moves  about 
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the  laboratory.  (For  the  very  first  set  of  observations,  some  kind  of 
initial  guess  .*ust  be  used.)  We  now  suppose  that  we  have  an 
approxima'  solution.  {u(rri),  v[m],  after  m  iterations.  We  define 

increme  {<5ufml  Sv^m\  Sw^}  by 


<5u(mj  _  u(m+l)  _  u(m)  ' 

St}”*  =  ~  it™*  y 

5w[m]  =  w{m+l)  -  wim] 


(3.1a) 


Since  the  superscripts  “(m)"  and  “(m+i)"  will  appear  very  frequently, 
from  now  on,  we  shall  replace  them  by  primes  ('  and  "  respectively). 
Then  (3.1a)  will  take  the  form 


<5u'  =  u"  -  u' 

Sv'  =  v"  -  v'  k 
Sw  =  w"  -  w’  > 


(3.1b) 


Newton's  method  consists  of  linearizing  the  problem,  and  solving  for 
the  increments  which  will  reduce  the  discrepancies  in  the  equations 
to  zero.  The  linearized  difference  equations  arising  from  (2.4)  are 


u  .  u'  +  2u'.  5u  =  a  1 
v' .  v  +  2v' .  Sv'  =  b  : 
w  .  w'  +  2  w' .  Sw'  =  c  > 


(3.2a) 


v' .  w'  +  Sv' .  w  +  v' .  Sw'  =  d 
vi .  u ’+  Sw' .  u  +  w' .  Su'  =  e  r; 
u  .  if  +  5u' .  if  +  u' .  Sif  =  J  y 


(3.2b) 


and.  by  (2.24)  and  (2.25),  we  have 
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hj  J  [  I  (B  -  a  u'  w'  i  +  !  (B  -  O  6v  w'\  +  i  (B  —  O  v'  Sw'  |  ] 

+  h12  [  | (B  -  O  w’  u'\  +  \(B-C\  Sw’  u'l  +  I  (B  -  Q  w'  Ju'l! 

+  h13  [  |  (B  -  a  u'  u'l  +  | (B -  Q  Su’  u'l  +  \  {B-Cj  u’  Sv'\] 

=  A[hl2  -  h13), 

h2l  [  1  (C  -  A)  v'  w'\  +  \(C- A)  8v'  w'  I  +  |  (C  -  A)  v'  Sw'  I  ] 

+  hr,2  [|(C-A)  w'  u'l  +  |(C-A)  SW  u'l  +  |(C-A)  w'  Su’l] 
+  3  f  |  (C  -  A)  u'  u'l  +  |(C  -  A)  Su’  v'  \  +  |  (C  -  A]  u  Sv'  I  ] 

=  Aih^ 3  -  hzJ, 

h3l  [|(A-B)  v'  w'\  +  |  (A  -  B)  Sv'  w'\  +  \  [A  -  B)  v’  Sw' I  ] 

+  h^2  [  |  (A  -  B)  w  u'  |  +  |  (A  -  Bj  Sw'  u’  I  +  |  (A  -  B)  w'  Su'  \  ] 

+  h33  [ | (A - B)  u'  u’l  +  |(A~B)  Su'  u'l  +  |(A-B)  u'  <5u'|] 

=  A(h5l  -  ft32)- 


(3.2c) 


(3. 2d) 


(3.2e) 


While  these  equations  are  quite  long,  they  nevertheless  reduce  to  nine 
simultaneous  linear  equations  for  the  nine  unknown  increments,  with 
coefficient  which  are  relatively  simple  linear  combinations  of  the 
components  of  the  previous  iterate.  We  may  further  simplify  the 
equations  as  follows.  First,  consider  equations  (3.2a)  and  (3.~2b).  Let 
us  put 


a-u  .  u 


d  -v' .  w' 


b-v' .  v' 


c  -  w  .  w 


e  =  e-w'.W  _  f-  u' .  v 
A  A 


(3.3) 


where  the  asterisk  (like  the  primes)  denotes  a  dependence  on  m;  and 
w’ritc 


-  n  - 
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5ll'  =  5'1 ,  if  A  W’  +  S 21  W’  A  u'  +  <5*3!  u'  A  if  ^ 


Sv'  =  S’  12  if  A  U)'  +  S22  Uf  A  U'  -i  (^33  u'  A  if  f. 

Suf  =  5”13  a  '  a  uf  +  5’23  uf  a  a'  +  <5'33  a'  a  v’  > 

(3.4) 

Then,  by  Fact  2  of  the  Appendix  and 
multiplying  (3.4),  we  see  that 

(2.2  i),  on  suitably 

scalar- 

^11  =  a*>  <^22  = 

&33  =  c*. 

(3.5a) 

and 

^23  +  ^32  = 

^31  +  6  13  =  ^ 

t 

(3.5b) 

^12  +  <^21 

which  reduces  (3.4)  to 


'N 

8u'  =  a*  if  a  uf  +  £  uf  a  a'  +  (e*  -  £)  a'  a  if 
Sv’  =  (/*  -  £)  if  a  uf  +  b*  uf  a  a'  +  77  a'  a  if  f , 
<5llf  =  (u’AlD'+fd'-IjlUJ'AU'+du'AO', 


(3.6) 


where  4  =  <521,  *7  =  ^32’  C  =  £'13-  (3.7) 

We  are  now  down  to  three  equations,  (3.2c)  -  (3.2e),  in  the 
remaining  three  unknowns,  q,  and  £.  From  (3.6),  we  get,  by  (A19), 
that 


Sv'  A  Uf  -  if  -  4)  [V  A  w']  A  W'  +  b*  ( W '  A  u')  A  W'  +  Tj  (u’  A  V')  A  W’ 

=  [ b *  ( uf 2)  -  r]  {v' .  w 7]  a'  +  (77  (uf .  u)  -  (/*  -  4)  (uf  2)J  v' 

+  [(/*  -  4)  (*>'  •  W)  -  b*  (uf  .  all  uf,  (3.8a) 

V'  A  5w'  =  4  v  A  (v'  A  w')  +  (d*  —  Tj)  V'  A  (llf  A  u)  +  C*  V'  A  (u'  A  Vl 

=  (c*  (if2)  -  fd*  -  77)  (if  .  tuO)  u'  +  [£  (if  .  w1)  -  c*  (u'  .  al]  if 

+  [(d*  -  7?)  (u'  .  a")  -  C  (a'2)]  uf.  (3.8b) 
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General  Headmount  Geometry 


Sw'  A  u'  =  C  (V  A  w)  A  u'  +  Id*  -  Tj)  ( W '  A  uO  A  u'  +  C*  (u'  A  1)1  A  u' 

=  I(d*  -  77)  (to' .  u)  -  c*  {u' .  o')]  u'  +  [c*  (u'2)  -  C  (uj'  .  u*)]  o' 

+  1C  (u'  .  o 0  -  (d*  -  77)  (u'2)]  to'.  (3.8c) 


IO'  A  <5u' 

=  a*  to'  a  (o' 

A  IOO  +  £  10'  A  (tO'  A 

u)  +  (e*  - 

£)  to'  A  (u'  A  o') 

=  He*  -  0  (©' 

.  w')  (to'2))  U’+  | 

[a*  (to'2)  - 

•  (c*  -  0  ( ttf ' 

.  uOl  0' 

+  [%  (to' .  ul  -  a*  (o' .  tv')]  to'. 

(3.8d) 

<5il'  A  O' 

=  a*  (o'  a  to”) 

A  O'  +  £  (lO'  A  UO  A 

0'  +  [e*  -  0  (u'  A  o')  A 

0' 

=  1$  (O'  .  ioO  - 

-  (e*  -  0  (u'2)l  U'  +  [(e*  -  0  (u' 

.  o')  -  a*  (o' 

.  toll  0' 

+  [a* (o'2) 

-  $  (u' .  o')]  1 o'. 

(3.8e) 

u'  A  8v' 

=  (/*-0u'a 

(o'  A  toT  +  b*  u’  A  (to'  A  u)  + 

7]  U'  A  (u'  A 

o') 

=  [77  (u' .  o')  - 

-  b*  (to' .  uO]  u'  +  [(f 

-  £)  (to' .  1 

-  77  (ix'2)I 

0' 

+  [b*  (u'2) 

-  (/*  -  £)  («'  •  trt]  to'. 

(3.8f) 

Let  us  now  write  [compare  (2.24)1 

D'i  =  l(B- 

-C)  0'  to'  I ,  D'2  = 

1  (B  -  C) 

to'  u'l, 

D'3  = 

=  l(B-C) 

u'  O'l; 

(3.9a) 

E'x  =  1  (C  - 

-A)  0'  to'  1 ,  E'2  = 

l(C-A) 

to'  u'l. 

Eg  = 

:  l(C-A) 

u'  o'l; 

(3.9b) 

Fj  =  1  (A  ■ 

-B)  0'  to'l,  F2  = 

1  (A  -  B) 

to'  u'l. 

^3  = 

=  l(A-B) 

u'  0'!; 

(3.9c) 

and,  similarly. 

D'  4  = 

(B  -  C)  .  u', 

D'5  =  (B  -  C)  .  o', 

D’g  =  (B  -  C)  .  to'. 

(3.10a) 

£4  = 

(C  -  A)  .  u', 

E'5  =  (C  -  A)  .  o', 

Eg  =  (C  -  A)  .  to'. 

(3.10b) 
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General  Headmount  Geometry 

F4  =  (A  -  B)  .  u\  F5  =  (A  -  B)  .  v\  F6  =  (A  -  B)  .  w'.  (3. 10c) 

Then  (3.2c)  -  (3.2e)  yield  that 

hu[[b*  w'2  +  c*  v'2  -  d*  v’ .  w]  D'4 

+  [t]  W .  u'  +  £v’ .  w’  -  c*  u' .  v'  -  (/*  -  #  w'2]  D'5 
+  [(d*  -  77)  u' .  v'  +  (/*  -  §  v' .  w'  -b*  w' .  u'  -  £  v' 2]  D'6| 

+  h12{[(d*  -  77)  w' .  u'  +  {e*  -  Q  v’ .  w’  -  c*  u' .  v’  -  $  w’2]  D\ 

+  [a*  w'2  +  c*  u'2  -  e*  w' .  u']  D'5 
+  [<fj  tu' .  u'  +  £  u' .  u'  -  a*  v’ .  w  -  (d*  -  77)  u'2]  D'6] 

+  h13{[<;  v’ .  w’  +  T]  u' .  v’  -  b*  w’ .  u’  -  (e*  -  Q  u’2]  D'4 

+  [(e*  -  Q  u' .  v'  +  (f  -  §  w' .  u'  -  a*  v' .  w'  -  77  u'2]  D’5 
+  [a*  v'2  +  b*  11' 2  -f  u' .  v ']  D'q) 

=  /l(hj2  ~  ^13)  ~  ( bii  1  iU  1  +  hj 2-D  2  +  ^13^  3) <  (3.11a) 

^{[b*  w' 2  +  c*  v’2  -  d*  u' .  id]  E\ 

+  [77  w' .  u'  +  £  v' .  id'  -  c*  u' .  u'  -  0*  -  #  id' 2]  E'5 
+  [(d*  -  77)  u' .  v'  +  (/*  -  $  v' .  10'  -  b*  u>' .  u'  -  C  V  2]  E'g] 

+  /^[[(d*  -  77)  id'  .  u'  +  (e*  -  £)  v' .  id'  -  c*  u' .  v'  -  £  u/2]  E’4 
+  [a*  tv'2  +  c*  u'2  -  e*  id’ .  u]  E'5 
+  [£  iu' .  u'  +  £  u' .  -  a*  v’ .  w'  -  (d*  -  77)  u'2]  E'6J 

(CONTINUED...) 
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General  Headmount  Geometry 


+  ^3 [ V  .  w'  +  77  u' .  v'  -  b*  w' .  u'  -  (e*  -  0  v' 2]  £ \ 

+  [(e*  -  £)  u' .  1/  +  (/*  -  #  u?' .  vl  -  a*  v' .  w- r\  u'2]  £'5 
+  [a*  v'2  +  b*u'2  -f  u' .  t>']  E'6} 

=  ^(^23  ”  ^21)  ~  (^21^  1  +  ^22^2  +  ^23^3^  (3.11b) 

u>'2  +  c*  u'2  -  d*  if .  uf]  F4 
+  [77  iu' .  u'+  £tf.  u/-c*u'.  if -(/*-£)  uf2]  F5 
+  [(d*  -  tj)  u' .  v' +  (f  -  §  v' .  tv'  -  b*  w' .  u'  -  £  v’2]  F6j 

+  b32[[(d*  -  Tj)  uf .  u'  +  [e*  -  0  *f  .  uf  -  c*  u’ .  if  -  £  to'2]  F4 

+  [a*  w'2  +  c*  u'2  -  e*  w' .  u']  F5 

+  [<£  uf .  u  +  £  u  ' .  if  -  a*  if .  uf  -  (d*  -  tj)  u'2]  F6| 

+  b.33|[^  if .  uf  +  77  u' .  if  -  b*  uf .  u'  -  (e*  -  0  V2]  F4 

+  [( e*  -  Q  u' .  v'  +  [j*  -  §  w' .  u'  -  a*  V  .  w'  -  q  u2]  F5 

+  [a*  v’2  +  b*  u'2  ~J*  u' .  if]  F6| 

=  4(h31- h32)  -  (h3 iFj  +  h32F2  +  h33F3).  (3.11c) 

These  rather  lengthy  equations  are  very  simply  solved.  They  take  the 
form 


AT 


4 

ri 

LC. 


=  5. 


(3.12) 


where 
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General  Headmount  Geometry 


M n  =  [h13  o' .  to' -  h12  to' 2]D'4  +  [hn  to’2  -  h13  to’ .  u']D'5 

+  [hl2w' •  u' -  hllif  .w’]D'6,  (3.13a) 

iVf  12  =  [h13  u' .  o'- h12  to* .  tt']D'4  +  [hn  to' .  u' -  h13  u'2]D'5 

+  u'2  -  hn  u' .  t/lDg,  (3.13b) 

Ml3  =  [h13  o'2  -  h12  o' .  to']D’4  +  [hn  o' .  to' -  h13  u' .  o']D'5 

+  [h12  u’ .  o'  -  hn  o'2]D'6,  (3.1  3c) 

M21  =  [/l23l,'*tO'-/T22tO’2]E'4+  [hgi  to'2  -  ^3  to’ .  IX']E'5 

+  to' .  u'  -  o' .  to'jE'g,  (3.13d) 

M22  =  [^23  tx'  .  O'  -  h22  U/  .  u']E'4  +  [hjj  to'  .  u'  -  ^3  u'2]E'5 

+  [^22  u' 2  - /^i  u' .  o’] Fg.  (3.13e) 

M23  =  [^23  u'2  -  ^2  *  “>']£'4  +  [^21  W'  •  to'  -  ^3  u'  .  0']E'5 

+  [h22u'.o'-h2iO'2]E’6.  (3. 13f) 

M31  =  [^33  ^'  •  to'  -  1132  10'2]F'4  +  [^31  to'2  —  hgs  to'  .  u']Eg 

+  [/I32  to' .  11' - /Tgi  o' .  to']F6.  (3.13g) 

M32  =  [^33  ^32  ^  •  u']F4  +  [^31^'-“' '  ^3  u'2]F5 

+  [/T32  u' 2  -  hsi  u' .  o']F6,  (3.13h) 

M33  =  [^33  v’2  -  ^32  v'  *  «>']F4  +  [^31  V.W’-h^u'.  O’]  Fg 

+  [h32u'.o'-h31o'2]F6.  (3.13i) 

and 
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g  1  -  ^(^12  “  ^13^  "  ^i|D  1  +  ^12^2  +  ^13^3^ 

-  hi  1 1 [ b*  W'2  +  C*  V'2  -  d*  V'  .  U>]  D'4 

+  [-  c*  u’ .  V  -j*  W’2]  D'g 
+  [d*  u' .  u'  + J*  v' .  w'  -  b*w’ .  u']  D'g  j 

-  h12{  [d*  w' .  u'  +  e*  V  .  w'  -  c*  u' .  t>']  D'4 

+  [a*  ur2  +  c*  u'2  -  e*  u>' .  u']  D's 
+  [-  a*  v’ .  w'  ~  d*  u'2]  D'6| 

-  h13{[-  b*  w' .  u'  ~  e*  v'2]  D\ 

+  [e*  u' .  v'  + j*  w' .  u!  -a*i/  .  u;']  D'5 
+  [a*  v’2  +  b*  u'2  -J*  u' .  *>']  D'6j,  (3.14a) 

92  -  A^3  -  ^2lJ  “  (^21^1  +  ^22^  2  +  ^23^3^ 

-  u;'2  +  c*  t>'2  -  d*  u' .  u>]  E'4 

+  [-c*u\  w'-y*  u>'2]  E's 
+  [d*  u' .  V  +  J*  v' .  XV’  -  b*  W  .  u]  E'gj 

-  [d*  w  •  u  +  ^  V  .  W  -  c*  u' .  v]  E4 
+  [a*  u>'2  +  c*  u'2  -  e*  w’ .  u']  E'5 

+  [-  a*  u'  .  w'  —  d*  u’2]  E'gj 

-  h23  j  [-  b*  w' .  u'  -  e*  v' 2]  JE'4 

+  [e*  u' .  t>'  + /*  w' .  u'  -  a*  i>' .  iu]  E'5 
+  [a*  i>'2  +  b*  u'2  -/  u' .  i>]  Eg}.  (3. 14b) 
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General  Headmoumt  Geometry 


g3  -  4(h31-  h32)  ~  (^31^1  +  ^32^ 2  +  ^33^3^ 

-  h31{[b*  tv'2  +  c*  u'2  -  d*  u' .  u>']  F4 

+  [-c*u'.  w'-/tu'2]  F5 
+  [d*  u' .  p'  +/1  v' .  tv’  -b*w' .  u']  F6J 

-  h32[ [d*  w' .  u’+  e*v' .  w'-c*  u' .  v ']  F4 

+  [a*  w’2  +  c*  u'2  -  e*  w' .  u)  F5 
+  [-  a*  v' .  w'  -  d*  u'2]  F6j 
-h 33{[- b- * .  u’ -  e- *1]  F4 

+  [e*  u' .  v'  +fw’.u’-a*if.  u/]  F5 
+  [a*  v'2  +  b*u' 2  -f  u' .  p']  Fg]. 


(3.14c) 


It  will  be  noticed  that  the  computation  of  these  coefficients  is  greatly- 
facilitated  by  the  repetition  of  the  same  combinations  of  parameters. 
In  particular,  if  we  write 


Fx  =  b*  w'2  +  c*  v'2  -d*  v' .  w’  ' 
F2  =  -  c*  u' .  v’ -J*  w’2  \ 
F3  =  d*  u' .  v’  +J*  v’ .  ip'  -  b*  w' .  u' 


(3.15a) 


Q\  =  d*  w' .  u’  +  e*  v’ .  w’  -  c*  u’ .  v’ 

Q'2  =  -  a*  w'2  +  c*  u'2  -  e*  ip' .  u’  >t 
03  =  -  a*  v’ .  W  -  d*  u’2  , 


(3.15b) 


Fj  =  -  b*  w’ .  u’ -  e*  v’2 
R2  =  e*  u' .  v’  +  j*  w’ .  u'  -  a*  v’  .w'  ►; 
F3  =  a*  v'2  +  b*  u'2  -j*  u' .  v' 


(3.15c) 
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then  (3.14)  becomes 

gl  =  A[hl2  -  h13)  -  (hjjD’j  +  h12D'2  +  h13D'3) 

-hn{Fl  D\  +  P'2  D's  +  F3  D'6) 

-  D4  +  Q'2  D5  +  03  D6> 

-  hl3{R\  D\  +  R'2  D'g  +  R’3  D’q).  (3.16a) 

g2  =  A[h23  -  h 21)  -  [h2lE'1  +  ^22^  2  +  ^23^3^ 

-  h21(P'1  E'4  +  F2  Eg  +  F3  E'6) 

-  haatO'j  E'4  +  Q'2  E'5  +  Q'3  E'6) 

-  h23(E'1  E4  +  R'2  E'5  +  R3  Eg),  (3.16b) 

g3  =  4(h31-  h32)  -  ih3lFl  +  h32F2  +  h33F3) 

-h3l[FlF4  +  F2F5+F3F6) 

-  ^Q\  ^4  +  02  F5  +  03  Ffl) 

-  /laatR'j  f4  +  R'2  f5  +  r3  Fg).  (3. 16c) 

Thereafter,  the  solution  of  the  system  of  three  equations  in  three 
unknowns  is  easy  to  perform. 

Of  course,  as  is  well  known,  Newton’s  method  is  asymptotically 
at  least  quadratically  convergent.  This  is  a  very  fast  rate  of 
convergence,  highly  satisfactory  for  most  purposes.  [See  §A4  of  the 
Appendix  for  more  detail;  we  shall  return  to  this  matter  in  §5.] 


4.  Operations  Count 


Let  us  now  suppose  that  the  equations  (2.4)  and  (2.25)  are 
reduced  to  linear  forms  (3.5),  then  (3.11),  and  finally  (3.12)  -  (3.1R): 
via  (2.5),  (2.22),  (3.1),  (3.3),  (3.4),  (3.7),  (3.9),  (3.10).  (A24),  and 
(A26);  and  solved  for  u.  v.  and  w  by  applying  Newtonian  iterations  as 
explained  above.  We  seek  to  establish  the  time  required  to  perform 
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the  necessary  operations  leading  up  to  the  process  of  solution  and, 
from  that  solution,  back  to  the  position  vector  s;  as  well  as  the  time 
required  by  each  iteration  of  Newton’s  method.  Since  different 
computers  have  different  timing  characteristics  for  floating-point 
operations  (flops),  and  since  these  usually  take  considerably  longer 
than  elementary  operations,  such  as  conditional  jumps,  ram  read  and 
store  commands,  integer  count  incrementations,  sign-changes,  shifts, 
etc.  (which,  anyway,  are  not  excessively  numerous  in  the  kind  of 
computation  considered  here);  all  we  need  to  do  is  to  count,  on  the 
one  hand,  all  the  multiplications  and  divisions  (M/D),  and,  on  the  other 
hand,  all  the  additions  and  subtractions  (a/s),  required  by  the 
computations  being  considered.  The  details  of  the  required 
derivations  are  assembled  in  §A3  of  the  Appendix. 

Before  the  readings  begin,  we  need  to  establish  the  values  uf  a,  b, 
c,  d,  e,  and/  in  (2.1);  and,  by  (2.22),  these  yield  the  value  of  A.  By 
(A33),  the  FLOP-count  for  computing  A  is  T>  -  9  M/D  +  5  a/s.  We  also 
need  2 A,  which  takes  1  a/s  more. 

Now,  each  time  that  we  take  a  set  of  location-readings,  we 
luenufy  duee  LED  landmark  position  vectors  A,  B,  and  C,  and  a  matrix 
K  of  direction-vectors  relative  to  the  headmounted  coordinate  system 
[see  (2.5)  -  (2.8)].  Some  computation  may  be  necessary  to  convert  raw 
directional  data  from  the  three  cameras  into  the  nine  components  ktj . 
but  we  shall  assume  that  this  is  done  very  quickly.  We  also  require  the 
vectors  B  -  C,  C  -  A,  and  A  -  B,  for  our  computations;  these  three 
vector  subtractions  take  a  trivial  9  A/S  to  obtain. 

The  process  of  inverting  K  to  obtain  its  reciprocal  H  takes,  by 
(A37),  31  =  27  M/D  +  18  A/S;  and  |k|  is  derived,  by  (A38),  as  a 
by-product  of  this  calculation  in  only  £>with  ^=2  m/d  more.  Then,  we 

need  the  quantities  A{hl2  ~  ^23^’  h(h 12  ~  ^23^’  A{hl2  ~  ^23^  in 
applying  (3.16)  to  the  iterations;  these  take  an  additional 
3  M/D  +  3  A/S. 

Once  we  have  performed  the  iterations  until  a  satisfactory  error- 
estimate  is  obtained,  we  must  use  our  final  iterates  u*,  v*,  and  w *,  say, 
to  obtain  s.  To  do  so,  we  must  first  compute  the  vectors  p*.  q*.  and 
r*.  using  (2.5);  by  (A41),  this  takes  =  27  m/d  +  18  A/S.  We  also 
need  the  vectors  B  -  v*,  C  -  w *,  and  A  -  u*;  this  takes  9  a/s  in  all. 
Then  we  can  proceed  in  two  ways. 

(a)  We  can  compute  the  approximation  s*  directly  from  (2.14). 
The  computation  of  the  common  denominator  Ip*  q*  r*  |  takes 
only  1  multiplication,  because  of  (2.23).  By  (A15)  and  (A33),  the  scalar 
numerators  take  3©  =  27  M/D  +  15  A/S.  to  form  the  triple  scalar 
products;  then  3  divisions  yield  the  scalar  coefficients,  and 
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3(3  m/d  .  2a/ s)  complete  the  evaluation.  Thus,  in  all,  we  need 
40  M/D +  21  A/S. 

(b)  We  can  solve  (2.11),  taking  advantage  of  the  simple  structure 
of  the  equations.  First,  we  respectively  eliminate  A  and  p  from  the 
first  pair  of  equations  in  (2.11a)  and  (2.11b).  yielding 

P2*V  -Pi*s2*  =  P2*(^i  "  ui*>  -Pi*(a2  "  “2*)- 

<?2*V  -  <Zl*s2*  =  ^2*(B1  -  vl">  - 

whence 

*  _  P2*ch*(Al  -  ul*)  -  P l*P2^Bl  -  -  Pl*qi*(A2  -  B2  +  v2*  -  “2*) 

Sl  "  P2*Pi* -Pi*Q2* 


p,*q2*(Ao  -  Un*)  -  p2*q1*(J52  -  Vo*)  -  p2*q2*(A1  -  BY  +  Uj*  -  ux*) 


>2  ~ 


Pi*<?2‘  ~P2*ch* 


(4.1) 


and  then  we  eliminate  v  from  the  last  two  equations  in  (2.11c), 
yielding 


r3*s2*  ~  r2*s3*  =  r3*^2  ~  w2*}  ~  r2*^3  _  w3*^< 


whence 


-  C2  +  m2*). 


(4.2) 


[The  temptation  must  be  resisted,  to  substitute  the  second  algebraic 
formula  in  (4.1)  into  (4.2);  this  leads  to  more  flops!]  An  operations 
count  on  (4.1)  and  (4.2)  shows  that  we  need  14  m/d  +  9  A/s  in  all. 
Clearly,  the  gain  in  using  the  second  method  is  considerable. 

Summing  up,  we  see  that  the  computations  required  at  every 
location-reading  require  altogether 

A  ~  73  M/D  +  66  A/S.  (4.3) 


Now  we  turn  to  the  computations  entailed  by  each  iteration * 
The  equations  to  be  solved  are  given  by  (3.12),  with  (3.13),  (3.15),  and 

(3.16).  By  (A40),  it  takes  Q  =  17  M/D  +11  A/S  to  solve  them  for  g,  7], 
and  £,  once  the  coefficients  are  known. 
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To  get  the  My,  we  first  need  to  calculate  the  six  scalar  products 

u'2,  v ’2.  w'2,  v' .  w\  w’  .  u'.  and  u' .  v'\  this  takes  6x(3  m/d  +  2  a/s). 
Then,  we  require  the  nine  coefficients  D'4,  D'5,  D'6,  E'4,  E'5,  E'6,  F4, 

Fs,  and  F6,  defined  in  (3.10);  this  takes  another  9  x  (3  m/d  +  2  a/s). 

Finally,  by  (3.13),  we  require  9  x  (9  M/D  +  5  A/s),  The  total  is 
126  M/D  +  75  A/S, 

To  get  the  gv  we  must  first  compute  the  six  parameters  a*,  b*, 
c*.  d*.  e *,  and  J*.  by  (3.3),  taking  6  m/d  +  6  a/s.  Next,  we  need  to 
compute  the  three  vector  products  v'  a  to’,  uj'a  u',  and  u' a  u';  this 
takes,  by  (A31),  S'F  =  18  m/d  +  9  a/S.  Then  we  need  the  eighteen 
coefficients  D\,  D'2.  D'3,  E\,  E'2,  E'3,  Fx,  F2,  and  F3,  defined  in  (3.9), 
and  P\,  P’2.  P'3.  Q\,  Q'2.  ©'3,  E'lt  E'2,  and  E'3,  defined  in  (3.15).  The 

former  take  9  x  (3  m/d  +  2  a/s)  =  27  m/d  +  18  a/s;  the  latter  take 
another  24  m/d  +  15  a/S.  Finaily.  by  (3.1 6),  we  require 
3  x  (15  m/d  +  12  a/s).  The  total  is  120  m/d  +  84  a/S. 

All  in  all,  we  need  246  M/D  +  159  a/ S  to  obtain  £,  77,  and  £,  for 
one  iteration.  Next,  we  need  to  apply  (3.6),  with  27  m/d  +  21  a/S,  to 
get  <5u'.  Sv’,  and  Sw'\  whence  we  finally  get  u",  v",  and  w"  in  9  a/s. 
Thus,  the  computations  in  every  iteration  of  the  Newton  process 
described  above  require  altogether 

®  =  273  M/D  +  189  A/S.  (4.4) 


5.  The  Restricted  System 


A  considerable  simplification  is  effected  by  restricting  every 
iterate  {u(m',  v^mK  u;^)  =  ju',  v',  w )  to  conform  to  the  exact  relations 
(2.4);  so  that 


u'2  =  a,  v'2  =  b,  w'2  =  c, 
and  v' .  w’  =  d,  w’ .  u  =  e.  u  .  v'  =  f. 

Then,  by  (3.3), 

a*  =  b*  =  c*  =  d*  =  e*  =  J*  =  0; 


(5.1) 

(5.2) 

(5.3) 
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and  (3.6)  becomes 

Su'  =  ^ui'au''(u'ai/ 

8v'  =  r\  u'  a  v’  -  t,  v’  a  w'  \  (5.4) 

Sw'  =  £  v'  A  U)'  -  7j  w'  A 

Thus,  by  (5.1)  and  (5.2),  the  equations  (3.13)  become 

2 1  =  ^^13  —  4  —  chis)D  5  +  (eh12-  dh^  ])D  g,  (5.5a) 

M12  =  C/^13  ~  ^12)D4  +  (e^ll  “  a^13^D5  +  (Qh12  “-/^l  l)D6’  (5.5b) 

=  (bh13  —  dh12)D4  +  (^^11  ~ 5  (/^  12  ~  6’  (5.5c) 

M2 j  =  (dh23  —  dhoo)E4  (c/i-2 1  —  5  (e^22~  dh22)£g.  ( 5 . 5 d ) 

-^22  =  (/^23  ~  e^22^4  +  (e^21  ~  a^23^  5  +  (a^22  “^21^6’  (5.5e) 

M23  =  (bh 23  -  dh22)£4  +  (dh21  +  (/T^  -  bh^JF'g,  (5.50 

M31  =  (dh33  -  ch32)F4  +  (ch31  -  eh33)F5  +  (^32*  dh3l)F6,  (5.5g) 
M32  =  (/h33  -  eh32)F4  +  (eh31  -  ah33)F5  +  (ah32  -Jh3l)F6,  (5.5h) 

M33  =  (bh33  -  dh32)F  4  +  (dh31  -fb33)F3  +  (/h32  -  bh31)F6,  (5.5i) 

and  (3.14)  becomes,  by  (5.3), 

£7  2  =  ^(^12  —  ^13)  —  (hjjD  2  +  2  "*■  ^13^  3) •  (5.6a) 

g2  =  h(h23  -  (^2 2)  ~  (^21^  1  +  ^22^2  +  ^23^3)*  (5.6b) 

g3  =  zUn31-  h32)  -  (h3lF 2  +  d32F 2  +  h33F 3).  (5.6c) 

We  can  now  analyze  the  reduced  operations  count,  much  as  we 

did  in  §4.  Before  readings  begin,  we  again  need  a,  b,  c,  d.  e,  and  /, 
from  which  we  compute  4  by  9  M/D  +  5  A/S.  We  do  not  need  2J. 
however,  saving  1  a/s. 

—  23  — 


General  Keacnrount  Geometry 


All  the  non-iterated  computations  needed  to  deal  with  each  new 
set  of  location  readings  in  the  general  case  are  still  needed  here;  they 
take  A  =  73  m/d  +  66  A/s.  However,  now  we  also  need  to  compute  the 
twenty-seven  coefficients  in  (5.5).  unfortunately  all  different,  each  of 
which  takes  2  m/d  +  1  a/s.  The  total  is 

A±  =  127  M/D  +  93  A/S.  (5.7) 

For  each  iteration,  the  same  form  of  equations  (3.12)  must  still 
be  solved,  for  q.  rf,  and  £;  once  the  coefficients  are  known,  this  again 
takes  17  m/d  +  1 1  a/s. 

To  get  the  My,  we  again  first  need  to  calculate  the  nine 
coefficients  D'4,  D'5,  D'g.  E'4,  £'5,  E'Q,  F4,  F5.  and  F6,  and  this  takes 

9  x  (3  m/d  +  2  A/S);  then,  by  (5.5),  another  9  x  (3  m/d  +  2  a/s).  The 
total  is  thus  54  m/d  +  36  a/s. 

To  get  the  gL,  we  first  need  the  nine  coefficients  D']t  D'2.  D'3. 
E\,  £'2.  E' 3.  F j.  F 2.  and  F3,  and  this  takes  9  x  (3  m/d  +  2  A/S):  then, 
by  (5.6),  another  3  x  (3  m/d  +  3  a/s)  are  needed.  Therefore,  in  all, 
we  need  107  m/d  +  74  a/S  to  obtain  <*,  r\ .  and  £,  for  one  iteration.  To 

get  (5u\  Su\  and  Sw’  from  (5.4)  requires  only  18  M/D  +  9  a/s:  whence 
we  finally  get  u  ".  v",  and  w  in  9  A/S.  Thus,  the  computations  in  every 
iteration  of  the  Newton  process  require 

F  =  125  M/D  +  92  A/S.  (5.8) 

This  is  less  than  half  of  the  count  in  (4.4). 


6.  Anticipatory  Initialization 


In  §3.  we  perfunctorily  suggested  that  the  initial  approximation, 
actually  is!0),  ul0),  u,0),  iu,(,))!.  should  be  “the  solution  obtained  (by 
Newton's  method)  for  the  most  recent  set  of  observations,  as  the 
wearer  of  the  headmounted  system  moves  about  the  laboratory."  With 
a  little  further  reflection,  we  can  improve  on  this.  As  the  observer 
wearing  the  headmounted  system  moves  about,  he  or  she  can  only  do 
so  by  continuing  the  previous  (translational  or  rotational)  motion  or  by 
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applying  a  new  force  or  torque  to  overcome  the  (linear  and  angular) 
inertia  of  his  or  her  body,  head,  and  helmet.  This  indicates  that,  by 
well-known,  elementary  dynamic  principles,  the  values,  and  first  and 
second  derivatives  with  respect  to  time,  of  all  the  components  of  s,  u, 
v,  and  w  will  be  continuous  and  differentiable;  though  the  third 
derivatives  may  be  discontinuous. 

Suppose  now  that  we  have  successive  observations,  made  at 
equal  time-intervals,  which  we  shall  denote  by 

rW  =  {AW,  bW,  dTi,  jtfr!h  t  =  0,  1,  2 _ ;  (6.1) 

where  A™,  BlT>.  and  dT  1  are  the  positional  vectors  of  the  landmark 
LEDS,  AM.  Bt*l,  and  C^,  and  is  the  matrix  of  observed  directions 
given  by  (2.5).  After  the  application  of  the  Newtonian  process  defined 
in  §3  or  §5  (or  by  direct  solution),  we  can  obtain  the  corresponding 
state,  which  we  shall  denote  by 

St*!  =  (s[T  1.  iM  yt*],  ujM).  (6.2) 

Let  us  suppose  that  we  have  obtained  the  states  S[T'  for  x  =  0,  1,  and  2, 
by  the  method  selected  above.  Then  we  may  anticipate,  by  the 
continuity  and  differentiability  of  the  values  and  of  the  first  and  second 

derivatives  with  respect  to  time  (and,  therefore,  with  respect  to  r. 
treated  as  a  continuous  variable),  and  by  Taylor’s  theorem,  that  a  good 
approximation  to  the  state  (and  therefore  an  excellent  initial  state  for 
Newton’s  method )  for  any  r  >  3  will  be  given  by  a  quadratic  fit  to  the 
three  preceding  states;  that  is,  if  we  temporarily  write  r0  =  x  -  2,  then 
there  are  constants  X,  Y,  and  Z,  such  that 

SM°>  =  X+  Y[r-  t0)  +  Z[x-  r0)2  =  X  +  2Y  +  4Z,  (6.3) 


where 


Si*" U  =  X+  YKt-  1) -t0]  +  Z(T-  1)  -f0]2  =  X+Y+Z 
Si *-2]  =  X  +  Y\[z  -  2)  -  TqJ  +  Z(r  -  2)  -  r0]2  =  X 
Si*"3!  =  X+  YI(r  -  3)  -  r0]  +  ZUt  -  3)  -  r0]2  =  X-Y+Z 


This  simplifies,  after  a  little  algebra,  to  yield 


(6.4) 
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X  =  sfr~2i  > 
Slr-H  _  &-3 1 

2  h 


sl^-n  -  2  SlT"21  +  S^-3] 

2 


(6.5) 


whence  (6.3)  becomes 

Slr!(0)  =  3SlT_ll  -  3SlT"21  +  S(T_31.  (6.6) 

The  use  of  this  initial  state;  that  is.  more  precisely,  of 

sW(O)  =  3s[r-l]  _  3s(r-2]  +  s(t-3]  ' 

UW(0)  =  3uIt-1]  _  3u!t-2]  +  ult-3] 

r;  (6.7) 

«lT)(0)  =  3ylT-ll  _  3vl t-2)  +  ^lr-31 
—  3y;[r-h  -  3u^(t-2'  +  -1 

will  ensure  a  super-fast  convergence  by  Newton’s  method  to  the  next 
state  S^,  using  the  formulae  already  developed  above. 


7.  Conclusions 


We  have  seen  that  the  equations  for  the  position  and  orientation 
of  the  general  three-camera  headmounted  location  system — (2.4). 
(2.14),  and  (2.25),  with  (2.5)  -  (2.8).  (2.21).  (2.23).  and  (2.24) — can 
be  solved  by  using  a  method  of  Newton’s  type.  It  is  advantageous  to 
restrict  the  method  to  require  (5.1)  and  (5.2),  when  the  increments 

in  the  iterates  are  given  by  (5.4);  then  the  (3  x  3)  system  (3.12)  must 
be  solved,  with  coefficients  given  by  (5.5)  and  (5.6). 

By  (5.7)  and  (5.8),  we  see  that  each  set  of  location  readings 
requires,  if  we  perform  m*  iterations  in  all  to  achieve  a  given  accuracy, 

+  m*  <B±  =  (127  +  125m*)  m/d  +  (93  +  92m*)  a/s;  (7.1) 
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or,  let  us  say.  approximately, 

(m*  +  1)(127  M/D  +  93  A/S).  (7.2) 

It  is  rather  difficult  to  estimate  directly  the  number  m*  of  iterations 
needed.  However,  by  the  formulae  (A54)  and  (A55),  we  do  know 
that — asymptotically  as  the  errors  tend  to  zero — if  the  improvement 
in  any  error  norm  (such  as  the  largest  absolute  error  among  the 
components  of  u,  v,  w,  and  s),  in  one  iteration,  is  by  a  factor  <p;  then 
the  improvement  in  the  next  iteration  will  be  by  the  factor  (p1 .  [In  the 
one-dimensional  case,  by  (A48),  we  know  that  there  is  a  constant  C, 
such  that,  as  ->  0, 


g(m+l) 

-  Ce<m>2; 

(7.3) 

whence 

girn+1) 

glm) 

(7.4) 

and  therefore 

tlm+2) 

£(m+l) 

~  c  £^m+h  ~  C2  £(m)2 

■g(m+l)l2 
.  fit™)  _  ' 

(7.5) 

In  the  multi-dimensional  case,  the  argument  is  similar.]  One  way  of 
describing  this  is  to  say  that,  in  each  iteration,  one  gains  twice  as 
many  significant  digits  as  in  the  previous  iteration.  Thus,  a  relatively 
short  experiment  will  indicate  the  number  of  iterations  needed  to 
achieve  the  required  accuracy.  We  note  that  the  greatest  change  in 
the  orientation  of  the  headmounted  system,  from  reading  to  reading, 

2tz 

will  be  of  the  order  of  1°  =  3^  <  0.02  radian.  Thus,  the  initial  iterate 

(u(0),  u(0)i  ujto)}  -yyjjj  differ  from  the  true  answer  {u,  v,  ui}  by  a  relative 
error  not  exceeding  2%.  If  we  apply  the  anticipatory  initialization 
outlined  in  §6  above,  we  may  expect  a  much  better  initialization,  and 
therefore  an  even  faster  convergence  to  the  required  accuracy. 

As  a  final  guiding  remark,  we  may  point  out  that  we  have 
assumed  300  readings  per  second.  For  real-time  operation,  this  gives 

us  3qq  second  =  3,333  psec.  to  perform  the  computations  in  (7.2).  If 

we  suppose  that  we  are  using  a  typical  1  MFLOPS  [1  million  floating¬ 
point  operations  per  second]  machine,  then  the  (m*  +  1)(127  m/d 

+  93  a/s)  operations  needed  take  some  220(m*  +  1)  psecs.  to 

3333 

perform.  This  means  that  we  have  available  time  for  some  -too"  -  1 
=  14  iterations.  This  should  be  ample. 
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Appendix 


ai.  Three-Dimensional  vector  algebra 


We  denote  ( column )  vectors  by  italic  boldface  characters  (e.g., 

A,  B,  C . p,  q,  r . x,  y,  z).  We  suppose  that  there  is 

a  “world  coordinate  frame"  of  reference,  given  by  the  origin  O  and 
a  right-handed  orthonormal  triad  {i,j,  fc}.  Relative  to  this,  we  write, 
for  example, 


or 


X  =  *  +  Z2j  +  %3k 

y  =  77i  x  +  rj2j+rj3fc 

z=  fii+fei+fefc. 


■*r 

>1' 

"Cl" 

X  = 

.  y  = 

^2 

.  z  = 

c2 

L  <^3  J 

_c3J 

(Al) 


(A2) 


The  transpose  of  any  matrix  is  denoted  by  appending  the  superscript 
suffix  T  .  For  instance,  if  x  is  a  column-vector,  its  row-vector  transpose 
is  xT  ;  thus,  e.g., 


*T  =  Ul  fe  hi  lA3l 

The  scalar  product  of  vectors  x  and  y  is  defined  to  be 

x.y  =  xTy  =  Z1V1  +  £2^2  +  £3%  •  (A4) 
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The  magnitude  (“length")  of  a  vector  z  is  denoted  by  I  z  I  and  defined 
to  be 

|z!2  =  z2  =  z  .  z  *  Cl2  +  C22  +  C32-  (A5) 

We  note  that,  since,  by  (Al), 


r 1  ■ 

■  0- 

’O' 

i  = 

0 

L0_ 

• 

.  J  = 

1 

0 

.  k  = 

0  : 
1 

(A6) 


it  follows  that 


i2  =  f  =  fc2  =  !,  i  ,  j  s  j  .  k  =  k  .  i  «  0.  (A7) 


It  also  follows  from  (A4)  that  |  z  |  =  0  if  and  only  if  Ci  =  C2  =  C3  =  0.  By 
(A4),  the  scalar  product  is  clearly  linear  in  both  its  factors. 

The  angle  between  vectors  x  and  y  is  denoted  by  9xy  and  is 
defined  by  the  relation 


x.y  =  y.x  =  !x|  lyicosfi^.  (A8) 

If  I  x  I  *  0,  |  y  |  *  0,  and  x  .  y  =  0.  we  say  that  the  vectors  x  and  y  are 
orthogonal;  and,  by  (A8),  6XJJ  =  i.e.,  the  vectors  are  mutually 

perpendicular.  Note  that  x.y  is  the  product  of  the  magnitude  of  y  and 
the  length  of  the  projection  of  x  onto  y  (or,  of  course,  uice  versa). 

The  vector  product  of  vectors  x  and  y  is  defined  to  be: 


x  a  y 


-  £3^2 
^1-^3 

L 


(A9) 


It  follows  immediately  that 


x  a  y  =  -y  ax. 


(A  10) 
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and  that 


Note  that  we  may  also  write  (A9)  as  a  formal  determinant: 

1  Si  Vi 
XAy  =  j  &  *12 

fc  S3  fl3 


(All) 


(A12) 


Further,  by  (A6)  and  (A9), 

i a  £  = j a j  =  kAk  =  0 
iAj  =  —j  a  £  =  fc 
j  A  fc  =  -  fc  A  j  =  i 

k  a  i  =  -  £  a  fc  =  j 

Fact  1.  x  a  y  =  (|x|  lylsin  0^) 

where  fcxy  denotes  a  unit  vector  (i.e.,  I  fcxy|  =  1)  perpendicular  to  the 
plane  of  x  and  y,  so  directed  that  {x,  y,  kxy }  form  a  right-handed  triad. 

Proof.  By  (A4)  and  (A9), 

x  .  (x  a  y)  =  ^(^2r?3  -  £3r72)  +  ^^3^1  ~  Si*73)  +  S3(SiT2  “  S2T?i)  =  0. 

and,  similarly,  x  .  (x  a  y)  =  0.  Thus  x  a  y  is  perpendicular  to  the 
plane  of  x  and  y.  Also,  by  (A4),  (A5),  (A8),  and  (A9), 

lx  Ay  |2  =  (£2r?3  -  S3*72)2  +  (S3n  1  ~  Si%)2  +  (£i?72 -S2t?i)2 
=  S2W  ~  ^3^3  +  S32H22  +  S32t?i2  -2SiS3^in3 
+  Si2%2  +  Si2T22  -2^2^  1^2  +  S22^i2 

{CONTINUED.,.} 
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=  (£i2  +  C22  +  ^32Hni2  +  r\22  +  H32) 

-  ($il7i  +  <52^2  +  ^3^2 

=  \x\2\y\2  -  \x\2\y\2  cos2  6^  =  \x\2 lyl2  sin2  0^ . 

This  proves  (A14)  to  within  a  factor  of  ±1.  Finally,  consider  the  case  of 
x  =  i  and  y  =  j,  when  x  a  y  =  i  a  j  =  k,  by  (A13).  Since  {£.  j,  k }  is  a 
right-handed  triad,  k  =  ky .  The  relationship  (A14)  holds  for  all  x  and 
y,  by  continuity  considerations. 

Note  that  the  expression  on  the  right  of  (Al*)  shows  that  the 
magnitude  of  x  a  y  equals  the  area  of  the  parallelogram  with  the  two 
vectors  x  and  y  as  adjacent  sides. 

We  now  consider  two  products  of  three  vectors.  The  triple 
scalar  product  is  defined  to  be  x  .  (y  a  z).  It  follows,  from  (A4),  (A9), 
and  (A12),  that 


x  .  (y  a  z)  =  (XAyj.z 


^2  *3 

'll  % 

Cl  c2  c3 


x  y  z  I.  (A15) 


Fact  2.  |  x  y  z  |  equals  the  volume  of  the  parallelepiped  with  x,  y, 

and  z  as  adjacent  sides  (with  the  usual  “right-hand  rule"  convention 
for  its  sign).  Thus,  I  x  y  z  I  =  0  if  any  two  of  the  vectors  are  equal. 

Proof.  We  have  seen  above  that  t  =  |  y  a  z  I  =  lyllzl  sin  6yz  is  the  area 

of  the  parallelogram  with  y  and  z  as  adjacent  sides;  and  the  vector  y  a 
z  is  perpendicular  to  the  plane  of  this  parallelogram.  Furthermore,  as 

we  have  also  seen  earlier,  x  .  t-  I  x  I  1 1|  cos  ®Xt’  which  is  the  magnitude 
of  t  times  the  projection  of  x  perpendicular  to  the  plane  of  y  and  z. 
Therefore,  x  .  (y  a  z)  equals  the  volume  of  the  parallelepiped  defined 
by  the  three  vectors.  By  (A15)  or  the  result  just  proven,  if  any  two  of 
the  vectors  are  equal,  the  parallelepiped  collapses  to  zero  volume. 

A  corollaiy  of  Fact  2  is: 

Fact  3.  +|xyz!=  +  |yzx|=  +  |zxy! 

=  -  I  z  y  x  |  =  -  |  y  x  z  I  =  -  |  x  z  y  \  .  (A  16) 
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Fact  4.  If  x  y  z  I  *  0,  then,  for  any  vector  t. 


t 


t  y  z  I  t  z  x  I  t  x  y  I 

- x+  i - ry  +  n - rz. 

x  y  z  x  y  z  x  y  z\ 


(A  17) 


Proof.  Since  |  x  y  z  I  *  0,  the  vectors  x.  y,  and  z  form  a  base; 
so,  certainly,  there  are  unique  K,  L,  and  M,  such  that 


Kx+Ly  +  Mz  =  t.  i'Al  8) 

Thus,  |  t  .  y  z  I  =  t .  (y  a  z)  =  K  |  x  y  z|,  since  I  y  y  z  I 
=  |  z  y  z  I  =  0,  by  Fact  2.  The  rest  of  (A17)  follows  similarly. 

Using  the  determinant  form  of  (A15),  we  see  that  (A17)  is 
nothing  else  than  the  famous  Cramer  Rule  for  the  equations  (A  18). 

We  now  turn  to  the  triple  vector  product,  x  a  (y  a  z). 


Fact  5.  XA(yAz)  =  (ZAy)AX  =  (x,z)y-(x.y)z.  (A19) 


Proof.  By  (A9), 


x  a  (y  a  z) 


£2^1  £2  ”  ^2^0  ”  ~  W1C3) 

^3^2^3  "  ^3^2)  ~  <5l(7?l<?2  “  ^2^0 
_^1^3^1  -  ^1 C3)  ~  ~  ^3^2}- 


fhitltl  +  ^2^2  +  *93^3)  ~  +  ^2^2  +  £3 ^3) 

^2^1  ^1  +  ^2^2  +  *53^  ~  ^2^1^1  +  ^2^2  +  £3^3) 
+  ^2^2  +  ^3^3^  “  ^3^1^1  +  £2^2  +  ^3%L 


which  proves  (A  19).  (Of  course,  since  y  a  z  is  clearly  perpendicular  to 
the  plane  of  y  and  z,  it  follows  that  x  a  (y  a  z)  is  in  the  plane  of  y  and  z, 
and  perpendicular  to  x.  Hence,  we  get  (A19)  at  once,  to  within  a 
scalar  factor;  however,  this  factor  is  rather  hard  to  determine.] 

Fact  6.  If  |  x  y  z  I  *  0,  then,  for  any  vector  t. 


t 


(t .  x) 
x  y  z 


(y  a  z)  + 


( t.y )  ,  ,  (t .  z) 

- 2 - (z  A  x)  +  -1 - (x  a  y) . 

x  y  z  x  y  z 


(A20) 
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Proof.  Again,  It  is  easy  to  see  that  the  triad  of  vectors  [y  a  z,  z  a  x. 
x  a  y]  form  a  base;  so  that  there  are  unique  P,  Q,  and  R ,  such  that 
t  =  P  [y  a  2)  +  Q  [z  a  x)  +  R  (x  a  y).  Thus,  t  .  x  =  P  |  x  y  2  I ,  as 
before;  and  the  rest  of  (A20)  follows  similarly. 

Fact  7.  (x  a  y)  .  (z  a  t)  =  (x  .  2)  (y  .  t)  -  [x  .  t)  [y  .  z).  (A2 1) 

Proof.  By  (A15),  with  Facts  3  and  4,  (x  a  y)  .  (2  a  t)  =  |  (x  a  y)  z  t  I 
=  |  t  (x  a  y)  z  I  =  t .  {(x  a  y)  a  2]  =  (t .  y)  (x  .  2)  -  (t .  x)  [y  .  2);  hence 
(API).  Similarly,  by  Fact  5,  we  get  (recovering  Fact  4)  that 

Fact  8.  (x  a  y)  a  (2  a  t)  =  |tx  y  I  2  -  |  x  y  z  \  t 

=  |t  x  2  I  y  -  |  t  y  z  I  x.  (A22) 


A2.  Reciprocal  of  a  (3  x  3)  matrix 


The  determinant  is  defined  hv 


kU 

^12 

k13 

K|  = 

^21 

^22 

^23 

^31 

fc32 

k33 

”  kllk22 

^33  " 

kllk23 

If  we  put 


G  = 


>  ^  'M2'v23'n31 
“  k12k21k33  +  kl3k21k32  "  k\3k22k3l' 

^22k33  “  ^23^32  k32kl3  “  k33k\2  ^12^23  ~  ^13^22 

^23^31  ~  ^21^33  k33kll  ~  k31k13  ^13^21  “  kll  ^23 

^31^12  _  ^32^11  ^11^22-^12^21 


^21^32  ~  ^22^31 
then  :t  is  easily  verified,  by  (A23),  that 


(A23) 


(A24) 
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KG 

=  GK  =  |K|  I, 

(A25) 

where  I  denotes  the  unit  matrix. 

Hence,  if  we  define 

"  hn 

^12 

^13 

H  = 

^21 

^22 

^23 

ii 

ft 

i 

P 

(A26) 

-  ^31 

^32 

^33  - 

then  we  get  that 

H  = 

=  jr1. 

(A27) 

A3.  ALGEBRAIC  OPERATION  COUNTS 


Turning  to  the  matter  of  operation  counts,  we  first  seek  the 
number,  (D,  of  flops  needed  to  evaluate  a  (3  x  3)  determinant.  Two 
methods  are  available  to  us.  (a)  We  can  use  the  formula  in  (A23), 
which  evidently  needs  6x2  M/D  (multiplications  and/or  divisions — in 
this  case,  multiplications)  and  5  A/S  (additions  and/or  subtractions). 
Thus, 


D(a)  =  12  M/D  +  5  A/S.  (A28) 

(b)  We  can  perform  the  Gaussian-elimination  kind  of  transformations 
to  reduce  the  determinant  to  diagonal  form. 
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NOTES:  ©  denotes  an  arbitrary  entry  In  the  tableau. 

[1]  By  Interchanging  a  pair  of  rows.  If  necessary — noting  that  any  such 
Interchange  entails  a  change  of  sign  in  the  determinant — try  to  make 
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the  (1 ,  1)  entry,  a.  non-zero.  If  this  is  impossible,  the  first  column  is  null 
and  therefore  the  determinant  must  be  zero:  terminating  the  process 
immediately. 

[21  2  divisions  by  a  are  required  in  the  first  row.  The  value  or  is  noted. 

[3]  2  x  (2  multiplications  and  2  A/S)  are  required  to  reduce  the  (2,  1)  and  (3,  1) 
entries  to  zero.  By  interchanging  last  two  rows,  if  necessary,  try  to  make 
the  (2,  2)  entry,  ji .  non-zero.  If  this  is  impossible,  the  determinant  must 
be  zero,  and  the  process  terminates  immediately. 

[4]  1  division  by  is  required  in  the  second  row.  The  value  /?  is  noted. 

[5]  1  multiplication  and  1  A/S  are  required  to  reduce  the  (3,  2)  entry  to  zero. 


The  value  of  the  determinant  is  a/Jy.  Unless  y  =  0,  we  need 
2  multiplications  to  obtain  this  value.  Combining  these  multiplications 
with  the  flops  listed  in  the  notes  above,  we  get 


2>(b)  =  10  M/D  +  5  A/S. 


(A30) 


(c)  We  can  use  the  triple  scalar  product  [see  (A15)].  The  generation  of 
the  vector  product  of  two  vectors  tskes  3  x  (2  multiplications  an d 
1  subtraction),  yielding 


V  =  6  M/D  +  3  A/S.  (A3 1) 

Then,  the  computation  of  the  triple  scalar  product,  by  way  of  the 
scalar  product,  takes  another  3  multiplications  and  2  additions, 
yielding 


2?(c)  =  9  m/d  +  5  A/S.  (A32) 

Thus,  method  (c)  is  slightly  preferable  and  should  be  adopted,  yielding 

T>  =  9  M/D  +  5  A/s.  (A33) 

Next,  we  seek  the  number,  30  of  flops  needed  to  find  the 

reciprocal  of  a  (3  x  3)  matrix.  Again,  there  are  two  likely  methods. 

(a)  We  can  use  (A24)  and  (A26),  for  which  we  require 
9  x  (2  multiplications,  1  subtraction,  and  1  division):  so  that 

3^a)  =  2)  +  27  M/D  +  9  A/S.  (A34) 

(b)  We  can  use  triple  Gaussian  elimination.  A  tableau  is  set  up,  with 
the  given  matrix  on  the  left  and  a  unit  matrix  on  the  right.  It  is  then 
transformed  as  follows. 
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(A3  5) 

&  again  denotes  an  arbitrary  entry  in  the  tableau. 

11]  By  Interchanging  a  pair  of  equations  (i.e.,  rows),  if  necessary,  try  to 
make  the  (1,1)  entry,  a.  non-zero.  If  this  Is  Impossible,  the  first  column 
Is  null  and  therefore  the  given  matrix  has  no  reciprocal. 

(2)  3  divisions  by  a  are  required  In  the  first  rowof  the  tableau.  The  'I'  in 
position  (1,4)  becomes  a-1,  a  case  of  ©. 


[31  2  x  (3  multiplications  and  3  A/S)  are  required  to  reduce  the  (2,  1)  and  (3,  1) 

entries  to  zero.  By  Interchanging  last  two  equations,  if  necessary,  try  to 
make  the  (2,  2)  entry,  /?,  non-zero.  If  this  is  impossible,  again  the  given 
matrix  has  no  reciprocal. 

(41  3  divisions  by  /?  are  required  in  the  second  rowof  the  tableau.  The  T  in 
position  (2,  5)  becomes  /T1,  a  case  of  ©. 

[51  3  multiplications  and  3  A/S  are  required  to  reduce  the  (3.  2)  entry  to  zero. 
Hopefully,  the  (3,  3)  entry,  y,  is  not  zero.  If  y=  0.  then  the  given  matrix 
has  no  reciprocal. 


16]  3  divisions  by  yare  required  in  the  third  row  of  the  tableau.  The  T  in 
position  (3,  6)  becomes  y-1.  a  case  of  S. 
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[7]  Begin  the  "back-substitution"  process.  2  x  (3  multiplications  and  3  A/S) 
are  required  to  reduce  the  (2,  3)  and  (1.3)  entries  to  zero.  The  right  half  of 
the  tableau  fills  with  ©. 

[8]  3  multiplications  and  3  A/ S  are  required  to  reduce  the  (1,2)  entry  to  zero. 

The  left  half  of  the  tableau  is  transformed  into  a  unit  matrix,  and  the 
right  half  becomes  the  required  reciprocal  matrix.  Combining  the 
flops  listed  in  the  notes  above,  we  get 

=  27  M/D +18  A/ S.  (A36) 

If  we  require  the  value  of  the  determinant  of  the  matrix,  too,  it  is  no 
extra  work  to  note  the  coefficients  a,  (3,  and  y,  as  we  go  along  in  (A33), 
and  the  determinant  value,  <x(3y,  is  obtained  by  only  two  additional 
multiplications.  Clearly,  the  second  method  is  again  preferable. 


yielding 

H  =  27  M/D  +  18  A/S. 

(A3  7) 

and 

®with  at  =  2  M/°- 

(A3  8) 

Veiy  analogous  is  the  problem  of  the  number  g  of  flops  required 
to  find  a  single  vector  solution  of  ->  system  of  three  equations  in  three 
unknowns.  Again,  Gaussian  elimination  is  a  prime  candidate,  and  the 
tableau  sequence  is  very  similar  to  (A35)  above. 


a  *  * 

©' 

1  *  $ 

©” 

[1] 

1  ©  © 

©  *  © 

© 

— > 

©  ©  © 

$ 

— > 

0  p  © 

*1* 

©  ©  © 

© 

©  ©  © 

© 

0  ©  © 

»*» 

- 

_ 

— 1 

[21 


1 

©1 

[31 

1 

© 

© 

©n 

[4) 

1 

© 

© 

*1* 

— » 

0 

1  © 

©  ' 

— * 

0 

1 

© 

© 

— > 

0 

1 

© 

© 

0 

© 

0 

0 

7 

© 

0 

0 

1 

© 

- 

_ 

w 

__ 

_ 

(5| 


1 

© 

0 

© 

[61 

1 

0 

0 

© 

—> 

0 

1 

0 

— > 

0 

1 

0 

a 

0 

0 

1 

© 

0 

0 

1 

[7) 


(A39) 


-37- 


General  Headmount  Geometry 


NOTES: 


[1]  3  divisions  by  a  are  required  In  the  first  rowof  the  tableau. 

[2]  2  x  (3  multiplications  and  3  A/S)  are  required  to  reduce  the  (2,  1)  and  (3,  1) 
entries  to  zero. 


[3]  2  divisions  by  {$  are  required  in  the  second  rowof  the  tableau. 

[4]  2  multiplications  and  2  A/S  are  required  to  reduce  the  (3.  2)  entry  to  zero. 

[5]  1  division  by  yls  required  in  the  third  rowof  the  tableau. 

[6]  Begin  the  “back-substitution"  process.  2  x  (1  multiplication  and  1  A/S) 
are  required  to  reduce  the  (2.  3)  and  (1,  3)  entries  to  zero. 

[7]  1  multiplication  and  1  A/S  is  required  to  reduce  the  (1,2)  entry  to  zero. 


We  get 


Q  =  17  M/D  +  11  A/S. 


(A40) 


It  is  easily  verified  that  the  multiplication  of  two  (3  x  3)  matrices 
will  require  9  x  (3  multiplications  and  2  additions),  yielding 

M  =  27  M/D  +  18  A/S.  (A41) 


A4.  Multi-Dimensional  Newt  on  method 


We  begin  with  the  one-dimensional  Newton  method  for 
iteratively  computing  the  solution  £  of  the  equation 

M  =  0.  (A42) 

Taylor's  expansion  is 

J\x  +  h)  =  fix)  +  h  j  h2  +  •  •  •  (A43) 

Newton’s  method  (sometimes  referred  to  as  the  Newton-Raphson 
method )  consists  of  selecting  a  suitable  initial  iterate  (guess),  x(0), 
approximating  and,  thereafter,  using  the  linearized  form  of  (A43)  to 
obtain  the  (m+l)-st  iterate,  from  the  m-th  iterate. 

j^m+1))  =  o  =  fxlrt)  4  (x*m+1>  -  x‘m))  •  (A44) 
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This  is  usually  written  in  the  form 

^trn+l)  -  ^m)  _  A^f—L 

If  we  write  -  £ 

for  the  error  in  the  m-th  iterate,  so  that 

x(m]  _  g  +  £(m)  ^  xtm+ 1)  =  £  +  £<m+I)f 

then  (A42)  -  (A44)  yield  that 


(A45) 

(A46) 

(A47) 


o  =  M)  +  £(m)  — ^  +  k^m)2  + 

0*5 


as2 


+  [f^1*  -^m>]  +  ••• 

=  eta+U-SM  {l  +  0[£^l!  -~^2  {i  +  Ol^lj: 
dg  0<r 

„  dj[Q  d2J[g] 

so  tliat,  if  — and  -—j,  ~  are  non-zero  (which  is  true  in  most  cases), 

os 

then 


^m+hMU.  _  I^fm)2  iLML  as  ^m)  _>  o 

05  2  0£2 

This  is  referred  to  as  quadratic  convergence. 

If  we  now  consider  a  system  of  n  equations  in  n  unknowns. 

Mi . Q  =  °.  i  =  1. - n; 

then  Taylor’s  expansion  becomes  (for  i=  1 . n) 


(A48) 


(A49) 
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/*(*  +  h)  =  fiix1  +  h-L . Xn  +  hn) 

"  df<M  i  £  £  .  .  32/,(*) 

=  AM  *  X »!,  T*T  +  5  X  X  hJ  h*  3^  + 


(A50) 


and  Newton's  method  becomes 


o  =  /,(*<"")  +  2  l*/"*1'  •. 

j—  1  J 


(A51) 


Writing 


P  (m)  _  y(m)  _  ; 
£J  XJ  qJ 


(A52) 


for  the  error  in  the  j-th  component  of  the  m-th  iterate,  we  can  use 
(A49)  -  (A51),  much  as  before,  to  yield 


0  =  V 

>iJ 


"  Wi . y  i  £  A  . « 


_  ,  L  V  V  p(rn)  p  (m)  _ 1 

Bxj  2hkJ  k 


+  2  (e/m+11  -  Cj(rnl) 


j-i 


l  3jc/ 


Jc=l 


k  5^- 


+ . . . 


-  X 


n  . w 


J=1 


dx, 


|l  +  o[maxj  I  ej™*  I  j| 


-  5  X  ,X  £Jlml  £k(ml  8  ~ Ixj Vxfc  ^  l1  +  °[maxJ  1  £j  !  ]}:  (A53) 


>1  k=  1 


0 y ^  £  £  j 

so  that,  if  at  least  one  each  of  the  partial  first  derivatives  - — 1 - — 

3  ^  ^  ^  ) 

and  of  the  partial  second  derivatives  — '  dx~dx - ~  are  non'zero  (which 

is  again  true  in  most  cases),  then 
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X  £jim+l) 


mv 


w  i  £  ^  . w 


>1 


_ .  ~  -  V  V  F  (m)  F  (m)  — - - - 

3x<  2  2^  2*  £J  k  dXi  dxi . 

J  J=1  k=l  J  K 


as  maxj  I  ej 


(m) 


0. 


(A54) 


Thus  the  quadratic  convergence  is  preserved. 

It  is  relatively  simple  to  verify  that  the  relation  (A54)  is  satisfied 
by  the  asymptotic  form 


maxj  I  ~  K  K^-m\  (A55) 

where  K  is  a  positive  constant  (depending  on  the  functions  jl . fn) 

and  k  is  another,  satisfying  0  <  k  <  1. 


A5.  the  Two-Dimensional  problem 


It  is  tempting  to  consider  a  simplified  problem,  in  which 
everything  occurs  in  the  plane  (two  dimensions),  rather  than  in  three- 
dimensional  space.  It  is  intuitively  plausible  to  consider  a  two-camera 
system,  in  this  case;  but  let  us  temporarily  retain  the  three  cameras. 
Referring  to  §2  and  adapting  to  two  dimensions,  we  find  that  (2.4) 
simplifies  to  only  five  equations;  namely, 


iP  =  iq2  +  iq2 
xP  =  iq2  +  iq2 

Up  =  uq2  +  uq2 


v. w  =  w.v  =  iquq  +  iquq  =  d 

w .  u  =  u.w  =  uqiq  +  uqiq  -  e  J 


(A56) 


whose  solutions  satisfy,  for  some  y. 
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U1  = 


V1  = 


e  cos  y  -  ^lac-  e2  sin  y 

VF 

d  cos  y  -  V  be  -  d2  sin  \fr 


U2  = 


VF  •  °2  =  Vc 

ioj  =  VF  cos  yf,  u>2  =  VF  sin  y 

The  last  equation  of  (2.4)  becomes 

[cf  -  de)2  =  (be  -  d2)  (ac  -  e2). 


e  sin  i/+  Vac  -  e2  cos 


V 


VF 

d  sin  y  +  Vbc  -  d2  cos  y 


(A57) 


(A58) 


which  is  a  relation  among  a,  b,  c,  d,  e,  and  / — redundant,  for  our 
purposes.  The  equations (2. 11)  reduce  to 


sx  +  Uj  +  Apx  =  A! > 

S2  +  U2  +  Ap2  =  A2 

51  +  wx  +  mi  =  Bj 

s2  +  u2  +  M2  = 

Sj  +  LUi  +  VTj  =  cx 

52  +  +  vr2  =  C2  > 


(A59) 


We  can  eliminate  A.  p.  and  v  as  before,  leaving  us  with  three  equations 
for  the  three  remaining  unknowns,  y,  s1,  and  s2.  Thus,  a  solution  is 
likely. 

By  contrast,  if  we  follow  intuition  and  limit  ourselves  to  only  two 
cameras,  then  (A56)  further  shrinks  to 


iP  -  +  ^2  =  b  ' 

Up  =  W2  +  UX 2 2  =  C  r ; 
v .  w  -  w .  v  =  vlw1  +  V2W2  =  d  „ 


(A60) 


whose  solutions  satisfy,  for  some  y. 
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vl  = 


d  cos  yr  -  Vbc"-d2~sin  y/  d  sin  y/+  V be  -  d2  cos  )//•  ] 

- - •  ^  = - 


Vc  ’  z  Vc" 

wl  =  4F  cos  y/.  ii>2  =  4c  sin  y/ 

[compare  (A57)j;  and  (A59)  diminishes  to 


(A61) 


sx  +  ut  +/iqx  = 

s2  +  u2  +  Wk  =  ^2 
S2  +  LOi  +  vr-j  = 

s2  +  ^  +  ^2  =  *-"2  " 


(A62) 


and,  when  we  eliminate  A  and  g,  we  are  left  with  two  equations  for  the 
same  three  unknowns,  y/,  slt  and  s2.  Thus,  a  solution  generally  cannot 

be  uniquely  determined.  Indeed,  for  any  choice  of  the  angle  y/,  we  can 
get  v  and  w  from  (A61);  whence  q  and  r  are  determined  by  the 
reduced  form  of  (2.5), 


q  =  ^  u+  k32U7| 
r  =  kr,3v+k33u>\' 

and  then  sx  and  s2  follow  from  [compare  (4.1)] 


(A63) 


q2fi(Bi  -  U])  -  qlr2(Cl  -  -  q1r1(B2  -  C2  +  u2  ~  tfg) > 

Q2rl-Qlr2 

qir2(B2  -  Ug)  -  <l2r4c2  -  v2 )  -  0f2r2tgl  -  Cx  +  Vl  -  Ui) 

<7l  r2  ~  ^2rl  > 


(A64) 


In  this  case,  intuition  turns  out  to  be  entirely  misleading! 
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